#include "quaternion4f.hpp"

using namespace sandbox::sdk;

const Quaternion operator * (const Quaternion  a, const Quaternion  b)
{
	//Q=w+ix+jy+kz
	//real=wW-xX-yY-zZ
	__m128 real=_mm_mul_ps(a.value,b.value);
	real=::sandbox::sdk::detail::shuffle<__m128,1,2,3,0>(real);
	//real=-(xX+yY+zZ-wW)
	//i=wX+Wx+yZ-zY
	__m128 im_i=_mm_mul_ps(
		::sandbox::sdk::detail::shuffle<__m128,0,1,2,3>(a.value)),
		::sandbox::sdk::detail::shuffle<__m128,1,0,3,2>(b.value));
	//j=wY+Wy+zX-xZ
	__m128 im_j=_mm_mul_ps(
		::sandbox::sdk::detail::shuffle<__m128,0,2,3,1>(a.value),
		::sandbox::sdk::detail::shuffle<__m128,2,0,1,3>(b.value));
	//k=wZ+zW+xY-yX
	__m128 im_k=_mm_mul_ps(
		::sandbox::sdk::detail::shuffle<__m128,0,3,1,2>(a.value),
		::sandbox::sdk::detail::shuffle<__m128,3,0,2,1>(b.value));
	
	//transpose this stuff
	//0 4 8 C
	//1 5 9 D
	//2 6 A E
	//3 7 B F real
	__m128 w0=::sandbox::sdk::detail::shuffle2<__m128,0,1,4,5>(im_i, im_j);
	__m128 w1=::sandbox::sdk::detail::shuffle2<__m128,2,3,6,7>(im_i, im_j);
	__m128 w2=::sandbox::sdk::detail::shuffle2<__m128,0,1,4,5>(im_k, real);
	__m128 w3=::sandbox::sdk::detail::shuffle2<__m128,2,3,6,7>(im_k, real);
	//0 2 8 A
	//1 3 9 B
	//4 6 C E
	//5 7 D F
	__m128 q0=::sandbox::sdk::detail::shuffle2<__m128,0,2,4,6>(w0, w2);
	__m128 q1=::sandbox::sdk::detail::shuffle2<__m128,1,3,5,7>(w0, w2);
	__m128 q2=::sandbox::sdk::detail::shuffle2<__m128,0,2,4,6>(w1, w3);
	__m128 q3=::sandbox::sdk::detail::shuffle2<__m128,1,3,5,7>(w1, w3);
	//0 1 2 3
	//4 5 6 7
	//8 9 A B
	//C D E F
	return Quaternion(_mm_add_ps(_mm_add_ps(q0,q1),_mm_sub_ps(q2,q3)));
}
